#pragma once

#include "i_base.h"

namespace idl
{
  class InterestPointBrief128 : public InterestPointBase<unsigned int, 4>
  {
  public:
    InterestPointBrief128();
    ~InterestPointBrief128();
  };

  class InterestPointBrief256 : public InterestPointBase<unsigned int, 8>
  {
  public:
    InterestPointBrief256();
    ~InterestPointBrief256();
  };

  class InterestPointBrief512 : public InterestPointBase<unsigned int, 16>
  {
  public:
    InterestPointBrief512();
    ~InterestPointBrief512();
  };

  /*Feature descriptor implementation for BRIEF*/
  class InterestPointDescriptorBrief
  {
  public:
    InterestPointDescriptorBrief() :_width(0), _height(0), _nbhd_width(0), _kernel_width(0){};
    InterestPointDescriptorBrief(const int width, const int height, float ipt_scale_base/*the base scale of Fast-Hessian feature*/);
    ~InterestPointDescriptorBrief();
    //! Describe all features in the supplied vector
    void describe(const float *const *integral, std::vector<InterestPointBrief128> &ipts, bool upright = true);
    void describe(const float *const *integral, std::vector<InterestPointBrief256> &ipts, bool upright = true);
    void describe(const float *const *integral, std::vector<InterestPointBrief512> &ipts, bool upright = true);
  private:
    inline void describeUpright(const float *const *integral, InterestPointBrief128 &ipt);
    inline void describeUpright(const float *const *integral, InterestPointBrief256 &ipt);
    inline void describeUpright(const float *const *integral, InterestPointBrief512 &ipt);
    int _width, _height, _nbhd_width, _kernel_width;
    int _offset[2048];
    float _base_scale;
  };

  //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
  /*BRIEF Feature descriptor matching functions*/
  template<typename FeatureType>
  int i_match_interestpoints_brief(const std::vector<FeatureType> &ipts_1, const std::vector<FeatureType> &ipts_2, std::vector<Pair<FeatureType, FeatureType> >& correspondences, double ratio_thre)
  {
    const int n = ipts_1[0].dim();
    correspondences.clear();
    correspondences.reserve(ipts_1.size());
    if (ipts_1.empty() || ipts_2.empty())
    {
      return 0;
    }
    int i, j, bestpos, type;
    unsigned int best, secondbest, dist;

    assert(n == ipts_2[0].dim());

    for (i = 0; i < (int)ipts_1.size(); i++)
    {
      best = secondbest = Constant<unsigned int>::MAX_VAL();
      bestpos = -1;
      type = ipts_1[i].type;
      for (j = 0; j < (int)ipts_2.size(); j++)
      {
        if (type != ipts_2[j].type)
        {
          continue;
        }

        dist = i_hamming_diff(ipts_1[i].descriptor, ipts_2[j].descriptor, n);

        if (dist < best) // if this feature matches better than current best
        {
          bestpos = j;
          secondbest = best;
          best = dist;
        }
        else
        {
          if (dist < secondbest) // this feature matches better than second best
          {
            secondbest = dist;
          }
        }
      }
      // If match has a best:secondbest ratio < ratio_thre then consider as a match
      if (bestpos >= 0 && i_div((double)best, (double)secondbest) < ratio_thre)
      {
        // push the match to the list
        correspondences.push_back(i_make_pair(ipts_1[i], ipts_2[bestpos]));
      }
    }
    return (int)correspondences.size();
  }

  template<typename FeatureType>
  int i_match_interestpoints_brief_unique(const std::vector<FeatureType> &ipts_1, const std::vector<FeatureType> &ipts_2, std::vector<Pair<FeatureType, FeatureType> >& correspondences, double ratio_thre)
  {
    const int n = ipts_1[0].dim();
    correspondences.clear();
    correspondences.reserve(ipts_1.size());
    if (ipts_1.empty() || ipts_2.empty())
    {
      return 0;
    }

    int i, j, bestpos, type;
    unsigned int best, secondbest, dist;

    assert(n == ipts_2[0].dim());

    std::vector<Pair<int, unsigned int> > record(ipts_2.size());

    for (i = 0; i < (int)ipts_2.size(); i++)
    {
      record.at(i).first = -1; /*empty - no match*/
      record.at(i).second = Constant<unsigned int>::MAX_VAL(); /*infinity*/
    }

    /*first pass*/
    for (i = 0; i < (int)ipts_1.size(); i++)
    {
      best = secondbest = Constant<unsigned int>::MAX_VAL();
      bestpos = -1;
      type = ipts_1[i].type;
      for (j = 0; j < (int)ipts_2.size(); j++)
      {
        if (type != ipts_2[j].type)
        {
          continue;
        }

        dist = i_hamming_diff(ipts_1[i].descriptor, ipts_2[j].descriptor, n);
        /*===================================*/

        if (dist < best) // if this feature matches better than current best
        {
          bestpos = j;
          secondbest = best;
          best = dist;
        }
        else
        {
          if (dist < secondbest) // this feature matches better than second best
          {
            secondbest = dist;
          }
        }
      }
      // If match has a best:secondbest ratio < ratio_thre then consider as a match
      if (bestpos >= 0 && i_div((double)best, (double)secondbest) < ratio_thre) /*bestpos can be negative, when all types are not consistent*/
      {
        // record the match
        if (best < record.at(bestpos).second)
        {
          record.at(bestpos).first = i;
          record.at(bestpos).second = best;
        }
      }
    }

    /*second pass*/
    for (j = 0; j < (int)ipts_2.size(); j++)
    {
      i = record.at(j).first;
      if (i >= 0)
      {
        // push the match to the list
        correspondences.push_back(i_make_pair(ipts_1[i], ipts_2[j]));
      }
    }

    return (int)correspondences.size();
  }

  template<typename FeatureType>
  int i_match_interestpoints_brief(const std::vector<FeatureType> &ipts_1, const std::vector<FeatureType> &ipts_2, std::vector<Pair<int, int> >& correspondences, double ratio_thre)
  {
    const int n = ipts_1[0].dim();
    correspondences.clear();
    correspondences.reserve(ipts_1.size());
    if (ipts_1.empty() || ipts_2.empty())
    {
      return 0;
    }
    int i, j, bestpos, type;
    unsigned int best, secondbest, dist;

    assert(n == ipts_2[0].dim());

    for (i = 0; i < (int)ipts_1.size(); i++)
    {
      best = secondbest = Constant<unsigned int>::MAX_VAL();
      bestpos = -1;
      type = ipts_1[i].type;
      for (j = 0; j < (int)ipts_2.size(); j++)
      {
        if (type != ipts_2[j].type)
        {
          continue;
        }

        dist = i_hamming_diff(ipts_1[i].descriptor, ipts_2[j].descriptor, n);

        if (dist < best) // if this feature matches better than current best
        {
          bestpos = j;
          secondbest = best;
          best = dist;
        }
        else
        {
          if (dist < secondbest) // this feature matches better than second best
          {
            secondbest = dist;
          }
        }
      }
      // If match has a best:secondbest ratio < ratio_thre then consider as a match
      if (bestpos >= 0 && i_div((double)best, (double)secondbest) < ratio_thre)
      {
        // push the match to the list
        correspondences.push_back(i_make_pair(i, bestpos));
      }
    }
    return (int)correspondences.size();
  }

  template<typename FeatureType>
  int i_match_interestpoints_brief_unique(const std::vector<FeatureType> &ipts_1, const std::vector<FeatureType> &ipts_2, std::vector<Pair<int, int> >& correspondences, double ratio_thre)
  {
    const int n = ipts_1[0].dim();
    correspondences.clear();
    correspondences.reserve(ipts_1.size());
    if (ipts_1.empty() || ipts_2.empty())
    {
      return 0;
    }
    int i, j, bestpos, type;
    unsigned int best, secondbest, dist;

    assert(n == ipts_2[0].dim());

    std::vector<Pair<int, unsigned int> > record(ipts_2.size());

    for (i = 0; i < (int)ipts_2.size(); i++)
    {
      record.at(i).first = -1; /*empty - no match*/
      record.at(i).second = Constant<unsigned int>::MAX_VAL(); /*infinity*/
    }

    /*first pass*/
    for (i = 0; i < (int)ipts_1.size(); i++)
    {
      best = secondbest = Constant<unsigned int>::MAX_VAL();
      bestpos = -1;
      type = ipts_1[i].type;
      for (j = 0; j < (int)ipts_2.size(); j++)
      {
        if (type != ipts_2[j].type)
        {
          continue;
        }

        dist = i_hamming_diff(ipts_1[i].descriptor, ipts_2[j].descriptor, n);

        if (dist < best) // if this feature matches better than current best
        {
          bestpos = j;
          secondbest = best;
          best = dist;
        }
        else
        {
          if (dist < secondbest) // this feature matches better than second best
          {
            secondbest = dist;
          }
        }
      }

      // If match has a best:secondbest ratio < ratio_thre then consider as a match
      if (bestpos >= 0 && i_div((double)best, (double)secondbest) < ratio_thre) /*bestpos can be negative, when all types are not consistent*/
      {
        // record the match
        if (best < record.at(bestpos).second)
        {
          record.at(bestpos).first = i;
          record.at(bestpos).second = best;
        }
      }
    }

    /*second pass*/
    for (j = 0; j < (int)ipts_2.size(); j++)
    {
      i = record.at(j).first;
      if (i >= 0)
      {
        // push the match to the list
        correspondences.push_back(i_make_pair(i, j));
      }
    }

    return (int)correspondences.size();
  }

  inline int i_match_interestpoints_brief128_unique(const std::vector<InterestPointBrief128> &ipts_1, const std::vector<InterestPointBrief128> &ipts_2, std::vector<Pair<InterestPointBrief128, InterestPointBrief128> >& correspondences, double ratio_thre)
  {
    correspondences.clear();
    correspondences.reserve(ipts_1.size());
    if (ipts_1.empty() || ipts_2.empty())
    {
      return 0;
    }

    int i, j, bestpos, type;
    unsigned int best, secondbest, dist;

    assert(8 == ipts_2[0].dim());

    std::vector<Pair<int, unsigned int> > record(ipts_2.size());

    for (i = 0; i < (int)ipts_2.size(); i++)
    {
      record.at(i).first = -1; /*empty - no match*/
      record.at(i).second = Constant<unsigned int>::MAX_VAL(); /*infinity*/
    }

    /*first pass*/
    for (i = 0; i < (int)ipts_1.size(); i++)
    {
      best = secondbest = Constant<unsigned int>::MAX_VAL();
      bestpos = -1;
      type = ipts_1[i].type;
      for (j = 0; j < (int)ipts_2.size(); j++)
      {
        if (type != ipts_2[j].type)
        {
          continue;
        }

        dist = i_hamming_diff4(ipts_1[i].descriptor, ipts_2[j].descriptor);
        /*===================================*/

        if (dist < best) // if this feature matches better than current best
        {
          bestpos = j;
          secondbest = best;
          best = dist;
        }
        else
        {
          if (dist < secondbest) // this feature matches better than second best
          {
            secondbest = dist;
          }
        }
      }
      // If match has a best:secondbest ratio < ratio_thre then consider as a match
      if (bestpos >= 0 && i_div((double)best, (double)secondbest) < ratio_thre) /*bestpos can be negative, when all types are not consistent*/
      {
        // record the match
        if (best < record.at(bestpos).second)
        {
          record.at(bestpos).first = i;
          record.at(bestpos).second = best;
        }
      }
    }

    /*second pass*/
    for (j = 0; j < (int)ipts_2.size(); j++)
    {
      i = record.at(j).first;
      if (i >= 0)
      {
        // push the match to the list
        correspondences.push_back(i_make_pair(ipts_1[i], ipts_2[j]));
      }
    }

    return (int)correspondences.size();
  }

  inline int i_match_interestpoints_brief128_unique(const std::vector<InterestPointBrief128> &ipts_1, const std::vector<InterestPointBrief128> &ipts_2, std::vector<Pair<int, int> >& correspondences, double ratio_thre)
  {
    const int n = ipts_1[0].dim();
    correspondences.clear();
    correspondences.reserve(ipts_1.size());
    if (ipts_1.empty() || ipts_2.empty())
    {
      return 0;
    }
    int i, j, bestpos, type;
    unsigned int best, secondbest, dist;

    assert(n == ipts_2[0].dim());

    std::vector<Pair<int, unsigned int> > record(ipts_2.size());

    for (i = 0; i < (int)ipts_2.size(); i++)
    {
      record.at(i).first = -1; /*empty - no match*/
      record.at(i).second = Constant<unsigned int>::MAX_VAL(); /*infinity*/
    }

    /*first pass*/
    for (i = 0; i < (int)ipts_1.size(); i++)
    {
      best = secondbest = Constant<unsigned int>::MAX_VAL();
      bestpos = -1;
      type = ipts_1[i].type;
      for (j = 0; j < (int)ipts_2.size(); j++)
      {
        if (type != ipts_2[j].type)
        {
          continue;
        }

        dist = i_hamming_diff4(ipts_1[i].descriptor, ipts_2[j].descriptor);

        if (dist < best) // if this feature matches better than current best
        {
          bestpos = j;
          secondbest = best;
          best = dist;
        }
        else
        {
          if (dist < secondbest) // this feature matches better than second best
          {
            secondbest = dist;
          }
        }
      }

      // If match has a best:secondbest ratio < ratio_thre then consider as a match
      if (bestpos >= 0 && i_div((double)best, (double)secondbest) < ratio_thre) /*bestpos can be negative, when all types are not consistent*/
      {
        // record the match
        if (best < record.at(bestpos).second)
        {
          record.at(bestpos).first = i;
          record.at(bestpos).second = best;
        }
      }
    }

    /*second pass*/
    for (j = 0; j < (int)ipts_2.size(); j++)
    {
      i = record.at(j).first;
      if (i >= 0)
      {
        // push the match to the list
        correspondences.push_back(i_make_pair(i, j));
      }
    }

    return (int)correspondences.size();
  }

  inline int i_match_interestpoints_brief256_unique(const std::vector<InterestPointBrief256> &ipts_1, const std::vector<InterestPointBrief256> &ipts_2, std::vector<Pair<InterestPointBrief256, InterestPointBrief256> >& correspondences, double ratio_thre)
  {
    correspondences.clear();
    correspondences.reserve(ipts_1.size());
    if (ipts_1.empty() || ipts_2.empty())
    {
      return 0;
    }

    int i, j, bestpos, type;
    unsigned int best, secondbest, dist;

    assert(8 == ipts_2[0].dim());

    std::vector<Pair<int, unsigned int> > record(ipts_2.size());

    for (i = 0; i < (int)ipts_2.size(); i++)
    {
      record.at(i).first = -1; /*empty - no match*/
      record.at(i).second = Constant<unsigned int>::MAX_VAL(); /*infinity*/
    }

    /*first pass*/
    for (i = 0; i < (int)ipts_1.size(); i++)
    {
      best = secondbest = Constant<unsigned int>::MAX_VAL();
      bestpos = -1;
      type = ipts_1[i].type;
      for (j = 0; j < (int)ipts_2.size(); j++)
      {
        if (type != ipts_2[j].type)
        {
          continue;
        }

        dist = i_hamming_diff8(ipts_1[i].descriptor, ipts_2[j].descriptor);
        /*===================================*/

        if (dist < best) // if this feature matches better than current best
        {
          bestpos = j;
          secondbest = best;
          best = dist;
        }
        else
        {
          if (dist < secondbest) // this feature matches better than second best
          {
            secondbest = dist;
          }
        }
      }
      // If match has a best:secondbest ratio < ratio_thre then consider as a match
      if (bestpos >= 0 && i_div((double)best, (double)secondbest) < ratio_thre) /*bestpos can be negative, when all types are not consistent*/
      {
        // record the match
        if (best < record.at(bestpos).second)
        {
          record.at(bestpos).first = i;
          record.at(bestpos).second = best;
        }
      }
    }

    /*second pass*/
    for (j = 0; j < (int)ipts_2.size(); j++)
    {
      i = record.at(j).first;
      if (i >= 0)
      {
        // push the match to the list
        correspondences.push_back(i_make_pair(ipts_1[i], ipts_2[j]));
      }
    }

    return (int)correspondences.size();
  }

  inline int i_match_interestpoints_brief256_unique(const std::vector<InterestPointBrief256> &ipts_1, const std::vector<InterestPointBrief256> &ipts_2, std::vector<Pair<int, int> >& correspondences, double ratio_thre)
  {
    const int n = ipts_1[0].dim();
    correspondences.clear();
    correspondences.reserve(ipts_1.size());
    if (ipts_1.empty() || ipts_2.empty())
    {
      return 0;
    }
    int i, j, bestpos, type;
    unsigned int best, secondbest, dist;

    assert(n == ipts_2[0].dim());

    std::vector<Pair<int, unsigned int> > record(ipts_2.size());

    for (i = 0; i < (int)ipts_2.size(); i++)
    {
      record.at(i).first = -1; /*empty - no match*/
      record.at(i).second = Constant<unsigned int>::MAX_VAL(); /*infinity*/
    }

    /*first pass*/
    for (i = 0; i < (int)ipts_1.size(); i++)
    {
      best = secondbest = Constant<unsigned int>::MAX_VAL();
      bestpos = -1;
      type = ipts_1[i].type;
      for (j = 0; j < (int)ipts_2.size(); j++)
      {
        if (type != ipts_2[j].type)
        {
          continue;
        }

        dist = i_hamming_diff8(ipts_1[i].descriptor, ipts_2[j].descriptor);

        if (dist < best) // if this feature matches better than current best
        {
          bestpos = j;
          secondbest = best;
          best = dist;
        }
        else
        {
          if (dist < secondbest) // this feature matches better than second best
          {
            secondbest = dist;
          }
        }
      }

      // If match has a best:secondbest ratio < ratio_thre then consider as a match
      if (bestpos >= 0 && i_div((double)best, (double)secondbest) < ratio_thre) /*bestpos can be negative, when all types are not consistent*/
      {
        // record the match
        if (best < record.at(bestpos).second)
        {
          record.at(bestpos).first = i;
          record.at(bestpos).second = best;
        }
      }
    }

    /*second pass*/
    for (j = 0; j < (int)ipts_2.size(); j++)
    {
      i = record.at(j).first;
      if (i >= 0)
      {
        // push the match to the list
        correspondences.push_back(i_make_pair(i, j));
      }
    }

    return (int)correspondences.size();
  }

  inline int i_match_interestpoints_brief512_unique(const std::vector<InterestPointBrief512> &ipts_1, const std::vector<InterestPointBrief512> &ipts_2, std::vector<Pair<InterestPointBrief512, InterestPointBrief512> >& correspondences, double ratio_thre)
  {
    correspondences.clear();
    correspondences.reserve(ipts_1.size());
    if (ipts_1.empty() || ipts_2.empty())
    {
      return 0;
    }

    int i, j, bestpos, type;
    unsigned int best, secondbest, dist;

    assert(8 == ipts_2[0].dim());

    std::vector<Pair<int, unsigned int> > record(ipts_2.size());

    for (i = 0; i < (int)ipts_2.size(); i++)
    {
      record.at(i).first = -1; /*empty - no match*/
      record.at(i).second = Constant<unsigned int>::MAX_VAL(); /*infinity*/
    }

    /*first pass*/
    for (i = 0; i < (int)ipts_1.size(); i++)
    {
      best = secondbest = Constant<unsigned int>::MAX_VAL();
      bestpos = -1;
      type = ipts_1[i].type;
      for (j = 0; j < (int)ipts_2.size(); j++)
      {
        if (type != ipts_2[j].type)
        {
          continue;
        }

        dist = i_hamming_diff16(ipts_1[i].descriptor, ipts_2[j].descriptor);
        /*===================================*/

        if (dist < best) // if this feature matches better than current best
        {
          bestpos = j;
          secondbest = best;
          best = dist;
        }
        else
        {
          if (dist < secondbest) // this feature matches better than second best
          {
            secondbest = dist;
          }
        }
      }
      // If match has a best:secondbest ratio < ratio_thre then consider as a match
      if (bestpos >= 0 && i_div((double)best, (double)secondbest) < ratio_thre) /*bestpos can be negative, when all types are not consistent*/
      {
        // record the match
        if (best < record.at(bestpos).second)
        {
          record.at(bestpos).first = i;
          record.at(bestpos).second = best;
        }
      }
    }

    /*second pass*/
    for (j = 0; j < (int)ipts_2.size(); j++)
    {
      i = record.at(j).first;
      if (i >= 0)
      {
        // push the match to the list
        correspondences.push_back(i_make_pair(ipts_1[i], ipts_2[j]));
      }
    }

    return (int)correspondences.size();
  }

  inline int i_match_interestpoints_brief512_unique(const std::vector<InterestPointBrief512> &ipts_1, const std::vector<InterestPointBrief512> &ipts_2, std::vector<Pair<int, int> >& correspondences, double ratio_thre)
  {
    const int n = ipts_1[0].dim();
    correspondences.clear();
    correspondences.reserve(ipts_1.size());
    if (ipts_1.empty() || ipts_2.empty())
    {
      return 0;
    }
    int i, j, bestpos, type;
    unsigned int best, secondbest, dist;

    assert(n == ipts_2[0].dim());

    std::vector<Pair<int, unsigned int> > record(ipts_2.size());

    for (i = 0; i < (int)ipts_2.size(); i++)
    {
      record.at(i).first = -1; /*empty - no match*/
      record.at(i).second = Constant<unsigned int>::MAX_VAL(); /*infinity*/
    }

    /*first pass*/
    for (i = 0; i < (int)ipts_1.size(); i++)
    {
      best = secondbest = Constant<unsigned int>::MAX_VAL();
      bestpos = -1;
      type = ipts_1[i].type;
      for (j = 0; j < (int)ipts_2.size(); j++)
      {
        if (type != ipts_2[j].type)
        {
          continue;
        }

        dist = i_hamming_diff16(ipts_1[i].descriptor, ipts_2[j].descriptor);

        if (dist < best) // if this feature matches better than current best
        {
          bestpos = j;
          secondbest = best;
          best = dist;
        }
        else
        {
          if (dist < secondbest) // this feature matches better than second best
          {
            secondbest = dist;
          }
        }
      }

      // If match has a best:secondbest ratio < ratio_thre then consider as a match
      if (bestpos >= 0 && i_div((double)best, (double)secondbest) < ratio_thre) /*bestpos can be negative, when all types are not consistent*/
      {
        // record the match
        if (best < record.at(bestpos).second)
        {
          record.at(bestpos).first = i;
          record.at(bestpos).second = best;
        }
      }
    }

    /*second pass*/
    for (j = 0; j < (int)ipts_2.size(); j++)
    {
      i = record.at(j).first;
      if (i >= 0)
      {
        // push the match to the list
        correspondences.push_back(i_make_pair(i, j));
      }
    }

    return (int)correspondences.size();
  }

} /*namespace idl*/